//typedef unsigned char byte;
//typedef unsigned char boolean;

#include <stddef.h>
#include <stdlib.h>
#include <stdint.h>
//#include <assert.h>             // define NDEBUG to remove assert
#include <Usb.h>
#include <ADB.h>
#include <i2cmaster.h>
#include "maxim.h"
#include "utils.h"
#include "PrintBuf.h"
#include "registers.h"
#include "packdata.h"

#define __DBG(n) Serial.println(n)

/*
  ================== GLOBALS =================
*/

PackData g_packData;
PrintBuf g_printer;

ADBconnection adb("tcp:6699");
char adbBuf[ADB_MAX_BUFSIZE+1];
char* helloAndroid = "Hello Android";

class StatsT {
public:
  uint16_t adbReadTot;  // techincally this is proxied data.
  uint16_t adbSentTot;
  uint16_t adbSentBad;
  uint32_t printNow;
public:
  StatsT():adbSentTot(0),adbSentBad(0),adbReadTot(0),
	  printNow(2000){}
  void print() {
    if (millis() < printNow) return;
    Serial.print(millis());Serial.print(" [STAT] "); 
    Serial.print(adbReadTot,HEX);Serial.print(";");
    Serial.print(adbSentTot,HEX);Serial.print(":");
    Serial.println(adbSentBad,HEX);
    printNow = millis()+2000;
  }
} Stats;

// Global status
enum {S_None=0,S_ADBinited,S_ADBconnected,S_GotINITCommand,S_BoardInited} Status = S_None;

//Discharge Monitoring routine
extern void dcmon(PackData*);

// initialize board.
extern boolean initBoard(PackData*);

extern void processADBmessages(char* adbBuf, uint16_t len);

/*
  ================== PROGRAM =================
*/

void adbWrite(uint8_t* data, uint16_t len)
{
  if (!adb.write(data, len)) {
    Stats.adbSentBad++;
  }
}

// Read data from ADB if available
void adbMessages()
{
  uint16_t len = adb.available();
  if (!len) {
    return;
  }
  Serial.print("len="); Serial.println(len);

  // read len data. we know it is going to be less than ADB_MAX_BUFSIZE.
  if (adb.read(len, (uint8_t*)adbBuf) != len) {
    Serial.println("adb.read!");
    return;
  }
  processADBmessages(adbBuf,len);
}

// Set status to indicate we got INIT command
void gotINITCommand()
{
  //  Serial.println("got init command");
  Status = S_GotINITCommand;
}

// initialize board
boolean initWrap()
{
  __DBG(11);
  if (!initBoard(&g_packData)) {
    Serial.println("Failed.");
    g_printer.print();
    return false;
  } 

  // enable scanning, read voltages and set nulls as well.
  if (!enableScan(true)) {
    g_printer.print();
    return false;
  }    

  if (!g_packData.readCellVoltages()) {
    g_printer.print();
    return false;
  }
  
  if (!g_packData.disableNullCells()) {
    g_printer.print();
    return false;
  }
  g_packData.printNumCells();
  g_packData.printCellVoltages(true);

  if (!setthr()) {
    g_printer.print();
    return false;
  }

  g_printer.add("maxim board initialized!");
  g_printer.print();
  Serial.println("maxim board initialized!");
  return true;
}

// Write to CELLEN register
boolean writeCellen()
{
  RegCellen regCellen(g_packData.getNumDevices());
  return regCellen.enableAllCells();
}

// Read STATUS register
boolean readStatus()
{
  RegStatus regStatus(g_packData.getNumDevices());
  if (!regStatus.read()) {
    return false;
  }

  regStatus.print();
}

// Clear STATUS register
boolean clearStatus()
{
  RegStatus regStatus(g_packData.getNumDevices());
  if (regStatus.clearStatus()) {
    Serial.println("Done!");
  } else {
    Serial.println("Failed.");
  }    
}

// Write to SCANCTRL register
boolean writeScanctrl()
{
  RegScanctrl regScanctrl(g_packData.getNumDevices());
  return regScanctrl.enable();
}

// Enable ADC scanning of cells.
boolean enableScan(boolean enableFullScan)
{
  if (enableFullScan) {
    if (!writeCellen()) {
      g_printer.add("Failed to write CELLEN register");
      return false;
    }
    //Serial.println("Wrote CELLEN register");
  }

  if (!writeScanctrl()) {
    g_printer.add("Failed to write SCANCTRL register");
    return false;
  }
  //Serial.println("Wrote SCANCTRL register");
  return true;
}  

// set thresholds on devices.
boolean setthr()
{
  return setthr(0,0,0,0);
}

boolean setthr(uint16_t uvSet, uint16_t uvClr, uint16_t ovSet, uint16_t ovClr)
{
  RegUVthrSet ruS(g_packData.getNumDevices());
  uvSet?ruS.setThreshold(uvSet):ruS.setDefaultThreshold();

  RegUVthrClear ruC(g_packData.getNumDevices());
  uvClr?ruC.setThreshold(uvClr):ruC.setDefaultThreshold();

  RegOVthrSet roS(g_packData.getNumDevices());
  ovSet?roS.setThreshold(ovSet):roS.setDefaultThreshold();

  RegOVthrClear roC(g_packData.getNumDevices());
  ovClr?roC.setThreshold(ovClr):roC.setDefaultThreshold();

  // Write to devices if already initialized.
  if (Status < S_BoardInited) {
    return true;
  }

  if (!ruS.writeAllDevices()) {
    g_printer.add("Failed to write UVTHRSET register");
    return false;
  }

  if (!ruC.writeAllDevices()) {
    g_printer.add("Failed to write UVTHRCLEAR register");
    return false;
  }
  if (!roS.writeAllDevices()) {
    g_printer.add("Failed to write OVTHRSET register");
    return false;
  }

  if (!roC.writeAllDevices()) {
    g_printer.add("Failed to write OVTHRCLEAR register");
    return false;
  }
  //Serial.println("Done.");
  return true;
}

// Read all cell voltages()
void readCellVoltages()
{
  if (g_packData.readCellVoltages()) {
    Serial.println("Done!");
  } else {
    Serial.println("Failed.");
    g_printer.print();
  }
}

void readRegister(char* p)
{
  uint8_t address=0;
  str2hex(p,address);

  Serial.print("Register:"); Serial.println(address,HEX);
  Register reg(g_packData.getNumDevices());
  reg.init(address);

  reg.readFromDevice();
  reg.print();
}

  
void printCellVoltages(boolean filterNoise)
{
  if (!g_packData.readCellVoltages()) {
    Serial.println("Failed to read.");
  }
  g_packData.printCellVoltages(filterNoise);
}

void disableNullCells()
{
  if (g_packData.disableNullCells()) {
    Serial.println("Done.");
  } else {
    Serial.println("Error disabling Null Cells.");
  }
    
}

inline void LEDhigh() {digitalWrite(13, HIGH);}
inline void LEDlow() {digitalWrite(13, LOW);}
static void playLED(int num)
{
  for (int i=0;i<num;i++) {
    digitalWrite(13, HIGH);   // set the LED on
    delay(100); 
    digitalWrite(13, LOW);    // set the LED off

    digitalWrite(13, HIGH);   // set the LED on
    delay(200); 
    digitalWrite(13, LOW);    // set the LED off
  }
}

/*
  ================== SETUP =================
*/
void setup()
{
  pinMode(13,OUTPUT);
  Serial.begin(115200);
  initialize();
}

boolean initialize()
{
  if (Status == S_BoardInited) return true;

  if (Status < S_ADBinited) {
    if (!adb.init()) {
      Serial.println("ADB init!");
      return false;
    }
    Status = S_ADBinited;
  }

  if (Status < S_ADBconnected) {
    if (!adb.connect()) {
      Serial.println("connect!");
      return false;
    }
    Status = S_ADBconnected;
    //    adbWrite((uint8_t*)helloAndroid,strlen(helloAndroid));
    Serial.println("ADB Connected.");
  }

  adb.poll();
  adbMessages();

  g_printer.printUsingADB();
  if (Status < S_GotINITCommand) {
    return false;
  } 
  Serial.println("BoardIniting...");

  if (Status < S_BoardInited) {
    if (!initWrap()) {
      Serial.println("Board Init failed!");
      g_printer.print();
      return false;
    }
    Status = S_BoardInited;
  }

  Serial.println("Board inited");

  LEDhigh();
  return true;
}

/*
  ================== MAIN LOOP =================
*/
void loop()
{
  if (!initialize()) {
    LEDlow();
    return;
  }

  adb.poll();
  adbMessages();

  dcmon(&g_packData);
}

